#include "../../../../../version.h"
#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_DYNAMICS_MRS_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
#pragma once
#define RL_TOOLS_RL_ENVIRONMENTS_L2F_PARAMETERS_DYNAMICS_MRS_H

#include "../../multirotor.h"

RL_TOOLS_NAMESPACE_WRAPPER_START
namespace rl_tools::rl::environments::l2f::parameters::dynamics{
    template<typename SPEC, typename = rl_tools::utils::typing::enable_if_t<SPEC::N == 4>> // This is a quadrotor
    constexpr typename ParametersBase<SPEC>::Dynamics mrs = {
            // Rotor positions
            {
                    {
                            0.1202081528017130834795622718047525268048,
                            -0.1202081528017130834795622718047525268048,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            0.1202081528017130834795622718047525268048,
                            0.1202081528017130834795622718047525268048,
                            0.0000000000000000000000000000000000000000
                    },
                    {

                            -0.1202081528017130834795622718047525268048,
                            0.1202081528017130834795622718047525268048,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            -0.1202081528017130834795622718047525268048,
                            -0.1202081528017130834795622718047525268048,
                            0.0000000000000000000000000000000000000000
                    }
            },
            // Rotor thrust directions
            {
                    {0, 0, -1},
                    {0, 0, -1},
                    {0, 0, -1},
                    {0, 0, -1},
            },
            // Rotor torque directions
            {
                    {0, 0, -1},
                    {0, 0, 1},
                    {0, 0, -1},
                    {0, 0, 1},
            },
            // thrust constants
            {
                    -1.7689986848125325291647413905593566596508,
                    0.0038360810526746032603218061751704226481,
                    0.0000013298253500372891536393180067499031
            },
            // torque constant
            0.016,
            // mass vehicle
            0.73,
            // gravity
            {0, 0, 9.81},
            // J
            {
                    {
                            0.0079113750000000000045519144009631418157,
                            0.0000000000000000000000000000000000000000,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            0.0000000000000000000000000000000000000000,
                            0.0079113750000000000045519144009631418157,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            0.0000000000000000000000000000000000000000,
                            0.0000000000000000000000000000000000000000,
                            0.0123065833333333343041493534997243841644
                    }
            },
            // J_inv
            {
                    {
                            126.4002780806117840484148473478853702545166,
                            0.0000000000000000000000000000000000000000,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            0.0000000000000000000000000000000000000000,
                            126.4002780806117840484148473478853702545166,
                            0.0000000000000000000000000000000000000000
                    },
                    {
                            0.0000000000000000000000000000000000000000,
                            0.0000000000000000000000000000000000000000,
                            81.2573216232504194067587377503514289855957
                    }
            },
            // action limit
            0.04, // T RPM time constant
            // hovering throttle (julia): sqrt((mass * 9.81/4 - thrust_curve[1])/thrust_curve[3]),
            1636.011825401871,
            {0, 2000},
    };

}
RL_TOOLS_NAMESPACE_WRAPPER_END
#endif